#include "EntechRobot.h"
#include "EntechConstants.h"

EntechRobot::EntechRobot(void)
{
	// BOB THE BUILDER STRIKES AGAIN
}

void EntechRobot::RobotInit() 
{
	// Actions which would be performed once (and only once)
	// upon initialization of the robot would be put here.

	// Acquire the Driver Station object
	//m_ds = DriverStation::GetInstance();
	//m_lcd = DriverStationLCD::GetInstance();	
	
	m_leftJoystick = new Joystick(c_joystickLeftPort);
	m_rightJoystick = new Joystick(c_joystickRightPort);
	
	m_compressor = new Compressor(c_compressorPressureSwitch, c_compressorRelayPort);
	m_entechDrive = new EntechDriveSubsystem();
	m_shooter = new ShooterSubsystem();
	m_prongs = new ProngsSubsystem();
	m_roller = new RollerSubsystem();
	
	//m_pneumaticsTest = new PneumaticsTest();
	//m_shooterTest = new ShooterTest();
	
	m_entechDrive->DoRobotInit();
	m_shooter->DoRobotInit();
	m_prongs->DoRobotInit();
	m_roller->DoRobotInit();
	
	m_shooter->SetSubsystems(m_prongs, m_roller);
	
	m_compressor->Start();   // TODO: Explain why this needed fixing 
	m_autoState = kStart;
		
	//m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"RobotInit() completed.");
	//m_lcd->UpdateLCD();
	//SmartDashboard::init();
}

void EntechRobot::AutonomousInit() 
{
	m_entechDrive->DoAutonomousInit();
	m_shooter->DoAutonomousInit();
	m_prongs->DoAutonomousInit();
	m_roller->DoAutonomousInit();
	
	//m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"AutonomousInit() completed.");
	//m_lcd->UpdateLCD();
}

void EntechRobot::TeleopInit() 
{
	m_entechDrive->DoTeleopInit();
	m_shooter->DoTeleopInit();
	m_prongs->DoTeleopInit();
	m_roller->DoTeleopInit();
	
	//m_pneumaticsTest->DoTeleopInit();
	//m_shooterTest->DoTeleopInit();
	
	//m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"TeleopInit() completed.");
	//m_lcd->UpdateLCD();
}

void EntechRobot::DisabledInit() 
{
	m_entechDrive->DoDisabledInit();
	m_shooter->DoDisabledInit();
	m_prongs->DoDisabledInit();
	m_roller->DoDisabledInit();
	
	//m_pneumaticsTest->DoDisabledInit();
	//m_shooterTest->DoDisabledInit();
	
	//m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"DisabledInit() completed.");
	//m_lcd->UpdateLCD();
}

void EntechRobot::AutonomousPeriodic() 
{
	// Assumption: Start robot with shooter in carry position with ball
	// Step1: open arms
	//        deploy prongs
	//        drive forward XX feet
	// Step2: Wait for drive to stop
	// Step3: Verify distance / Correct distance
	// Step4: Wait for correction to finish
	// Step5: Shoot
	// Step6: Done
	switch (m_autoState) {
	case kStart:
		m_autoState = kStep1;
		break;
	case kStep1:
		m_roller->SetPosition(RollerSubsystem::kBack);
		m_prongs->SetPosition(ProngsSubsytem::kDown);
		m_entechDrive->DriveByDistanceInInches(120.,120.);
		m_autoState = kStep2;
		break;
	case kStep2:
		if (m_drive->IsAutoDriveFinished()) {
			m_autoState = kStep3;
		}
		break;
	case kStep3:
#if 0
		dist = m_sonar->GetDistanceInInches();
		if ((dist > c_maxShooterDistance) || (dist < c_minShooterDistance)) {
			dist = dist - (c_maxShooterDistance + c_minShooterDistance)/2.0;
			m_entechDrive->DriveByDistanceInInches(dist,dist);
			m_autoState = kStep4;
		} else {
			m_autoState = kStep5;
		}
#else
		m_autoState = kStep5;
#endif
		break;
	case kStep4:
		if (m_drive->IsAutoDriveFinished()) {
			m_autoState = kStep5;
		}
		break;
	case kStep5:
		m_shooter->SetShooterState(ShooterSubsystem::kFire);
		m_autoState = kDone;
		break;
	case kDone:
		break;
	}

	m_entechDrive->DoAutonomousPeriodic();
	m_shooter->DoAutonomousPeriodic();
	m_prongs->DoAutonomousPeriodic();
	m_roller->DoAutonomousPeriodic();
}
	
void EntechRobot::TeleopPeriodic() 
{
	m_entechDrive->DoTeleopPeriodic();
	m_shooter->DoTeleopPeriodic();
	m_prongs->DoTeleopPeriodic();
	m_roller->DoTeleopPeriodic();
	
	//m_pneumaticsTest->DoTeleopPeriodic();
	//m_shooterTest->DoTeleopPeriodic();
}

void EntechRobot::DisabledPeriodic()  
{
	m_entechDrive->DoDisabledPeriodic();
	m_shooter->DoDisabledPeriodic();
	m_prongs->DoDisabledPeriodic();
	m_roller->DoDisabledPeriodic();
}

START_ROBOT_CLASS(EntechRobot);
